#include "speed_cnrt.h"
#include "device.h"
#include "modbus.h"

//! Cointains data for timer interrupt.
SpeedRampData srd_y = {0};
SpeedRampData srd_z = {0};
SpeedRampData srd_x = {0};
SpeedRampData srd_m = {0};

void speed_cntr_Move(SpeedRampData *srd, long step, unsigned int speed)
{
    if (step == 0)
        return;

    // Set direction from sign on step value.
    if (step < 0)
    {
        srd->dir = CCW;
        srd->step_offset = -1;
        step = -step;
    }
    else
    {
        srd->dir = CW;
        srd->step_offset = 1;
    }

    // 最大速度对应的速度表索引
    // n = speed^2 / (2*alpha*accel)
    srd->max_index = (ulong)speed * speed * (1 / (2 * ALPHA * ACCEL * 100));
    if (srd->max_index > (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1)
        srd->max_index = (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1;
    if (srd->first_stage_speed > (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1)
        srd->first_stage_speed = (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1;
    if (srd->second_stage_speed > (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1)
        srd->second_stage_speed = (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1;

    srd->speed_index = 0;
    srd->step_left = step;
    srd->run_state = MOTION_STATE_RUN;

    srd->hit_home = 0;
    srd->running = 1;
    srd->need_cleanup = 0;
    srd->start_time = SystemTick();
    srd->step_count = 0;
    srd->init();
}

void speed_cntr_update(SpeedRampData *srd)
{
    if (!srd->running)
        return;

    if (srd->coder_self)
    {
        if (++srd->micro_step < (srd->micro >> 1))
            return;
        srd->micro_step = 0;
    }

    if (srd->run_state == MOTION_STATE_STOP)
    {
        srd->stop_timer();
        srd->need_cleanup = 1;
    }
    else
    {
        // 更新已经运行的步数和剩余步数
        srd->step_count++;
        srd->step_left--;

        srd->start_timer(FREQ_TABLE(srd->speed_index));
        // 走一步, 调用更新函数, 用以检查传感器状态, 更新剩余步数等
        srd->update();

        if (srd->step_left <= 0)
        {
            srd->run_state = MOTION_STATE_STOP;
        }
        else
        {
            // 更新下一步速度
            if (srd->speed_index > srd->step_left)
                srd->speed_index -= srd->speed_index / (ushort)srd->step_left;
            else if (srd->speed_index > srd->max_index)
                srd->speed_index--;
            else if (srd->speed_index < srd->max_index)
                srd->speed_index++;
        }
    }
}
